﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using SimpleMessaging;
using JsonExSerializer;
using System.IO;

namespace FTFSmartCar
{
    public partial class SystemControlForm : Form
    {
        //this queue will be used by the main GUI form.  It will periodically check to see if there are bytes to TX out
        public Queue<byte []> MsgToTx = new Queue<byte []>();


        public SystemControlForm()
        {
            InitializeComponent();
            MsgToTx.Clear();
            SystemStateComboBox.SelectedIndex = 0;
            UpdateTextLabels();
        }

        private void SystemControlForm_FormClosed(object sender, FormClosedEventArgs e)
        {
            SaveFormSettings();
        }

        private void SystemControlForm_FormClosing(object sender, FormClosingEventArgs e)
        {
            e.Cancel = true;
        }

        private void ChangeSystemState_Click(object sender, EventArgs e)
        {
            GlobalMessages.SetSystemStateMessage OutMsg = new GlobalMessages.SetSystemStateMessage();

            switch ((String)SystemStateComboBox.Items[SystemStateComboBox.SelectedIndex])
            {
                default:
                case "SYSTEM_STATE_INIT":
                    OutMsg.MsgStruct.NewSystemState = FTFSmartCarMessages.SystemStates.SYSTEM_STATE_INIT;
                    break;

                case "SYSTEM_STATE_IDLE":
                    OutMsg.MsgStruct.NewSystemState = FTFSmartCarMessages.SystemStates.SYSTEM_STATE_IDLE;
                    break;

                case "SYSTEM_STATE_AUTO_NV":
                    OutMsg.MsgStruct.NewSystemState = FTFSmartCarMessages.SystemStates.SYSTEM_STATE_AUTO_NV;
                    break;
            }

                MsgToTx.Enqueue(OutMsg.ToByteArray());
        }

        private void SW1_Click(object sender, EventArgs e)
        {
            FTFSmartCarMessages.SimulateButtonPress MySimButton = new FTFSmartCarMessages.SimulateButtonPress();
            MySimButton.MsgStruct.ButtonPress = 0x01;
            MsgToTx.Enqueue(MySimButton.ToByteArray());
        }

        private void SW2_Click(object sender, EventArgs e)
        {
            FTFSmartCarMessages.SimulateButtonPress MySimButton = new FTFSmartCarMessages.SimulateButtonPress();
            MySimButton.MsgStruct.ButtonPress = 0x02;
            MsgToTx.Enqueue(MySimButton.ToByteArray());
        }

        private void SW3_Click(object sender, EventArgs e)
        {
            FTFSmartCarMessages.SimulateButtonPress MySimButton = new FTFSmartCarMessages.SimulateButtonPress();
            MySimButton.MsgStruct.ButtonPress = 0x04;
            MsgToTx.Enqueue(MySimButton.ToByteArray());
        }

        private void SW4_Click(object sender, EventArgs e)
        {
            FTFSmartCarMessages.SimulateButtonPress MySimButton = new FTFSmartCarMessages.SimulateButtonPress();
            MySimButton.MsgStruct.ButtonPress = 0x08;
            MsgToTx.Enqueue(MySimButton.ToByteArray());
        }


        void UpdateTextLabels()
        {
            SteeringGainLabel.Text = "SteeringGain : " + String.Format("{0:0.000}", (double)SteeringGainControlTB.Value / (double)256) + " [FIXED_7_8] " + SteeringGainControlTB.Value + " [SIGNED_WORD]";
            DriveGainLabel.Text = "DriveGain : " + String.Format("{0:0.000}", (double)DriveGainGainControlTB.Value / (double)256) + " [FIXED_7_8] " + DriveGainGainControlTB.Value + " [SIGNED_WORD]";
            MaxDriveSpeedLabel.Text = "MaxDriveSpeed: " + String.Format("{0:0.000}", (double)MaxDriveSpeedTB.Value / (double)256) + " [FIXED_7_8] " + MaxDriveSpeedTB.Value.ToString() + " [SIGNED_WORD]";
            IntegralGainLabel.Text = "IntegralGain: " + String.Format("{0:0.000}", (double)IntegralGainTB.Value / (double)256) + " [FIXED_7_8] " + IntegralGainTB.Value.ToString() + " [SIGNED_WORD]";

            CentroidDifferentialGainLabel.Text = "DifferentialGain: " + String.Format("{0:0.000}", (double)CentroidDifferentialGainTB.Value / (double)256) + " [FIXED_7_8] " + CentroidDifferentialGainTB.Value.ToString() + " [SIGNED_WORD]";

            if (CentroidFilterLengthTB.Value< 2)
            {
                CentroidFilterLengthLabel.Text = "Centroid Filter Disabled";
            }
            else
            {
                CentroidFilterLengthLabel.Text = "Centroid Filter Length : " + CentroidFilterLengthTB.Value;
            }

            
            if(IRFaultTickLimitTB.Value == 0)
            {
                MaxAllowableIRSignalFloorLabel.Text = "IR Fault Stop Disabled";
                MinAllowableIRSignalStrengthLabel.Text = "IR Fault Stop Disabled";
                IRFaultTickLimitLabel.Text = "IR Fault Stop Disabled";
            }
            else
            {
                IRFaultTickLimitLabel.Text = "IR Fault Tick Limit : " + IRFaultTickLimitTB.Value + " [" + (IRFaultTickLimitTB.Value * .02) + " seconds]";
                MaxAllowableIRSignalFloorLabel.Text = "Max Allowable IR Signal Floor : " + MaxAllowableIRSignalFloorTB.Value;
                MinAllowableIRSignalStrengthLabel.Text = "Min Allowable IR Signal Strength: " + MinAllowableSignalStrengthTB.Value;
            }
           
         }

        private void SteeringGainControlTB_Scroll(object sender, EventArgs e)
        {
            UpdateTextLabels();
            UpdateAutoNVParameters();
        }

        private void DriveGainGainControlTB_Scroll(object sender, EventArgs e)
        {
            UpdateTextLabels();
            UpdateAutoNVParameters();
        }

        private void MaxDriveSpeedTB_Scroll(object sender, EventArgs e)
        {
            UpdateTextLabels();
            UpdateAutoNVParameters();
        }

        void UpdateAutoNVParameters()
        {
            FTFSmartCarMessages.SetAutoNVParameters MyAutoNV = new FTFSmartCarMessages.SetAutoNVParameters();

            if(DisableAutoSteeringCB.Checked == true)
            {
                MyAutoNV.MsgStruct.DisableAutoSteering = 1;
            }
            else
            {
                 MyAutoNV.MsgStruct.DisableAutoSteering = 0;
            }

            if(DisableAutoDriveCB.Checked == true)
            {
                MyAutoNV.MsgStruct.DisableAutoSpeed = 1;
            }
            else
            {
                 MyAutoNV.MsgStruct.DisableAutoSpeed = 0;
            }

            MyAutoNV.MsgStruct.MaxDriveSpeed = (short)MaxDriveSpeedTB.Value;
            MyAutoNV.MsgStruct.SteeringCtrlGain = (short)SteeringGainControlTB.Value;
            MyAutoNV.MsgStruct.SpeedCtrlGain =(short)DriveGainGainControlTB.Value;

            if (ResetIntegralCB.Checked == true)
            {
                MyAutoNV.MsgStruct.SteeringIntegralGain = 0;
            }
            else
            {
                MyAutoNV.MsgStruct.SteeringIntegralGain = (short)IntegralGainTB.Value;
            }

            MyAutoNV.MsgStruct.SteeringDifferentialGain = (short)CentroidDifferentialGainTB.Value;
            
            MyAutoNV.MsgStruct.CentroidFilterLength = (byte)CentroidFilterLengthTB.Value;
            MyAutoNV.MsgStruct.MaxAllowableSignalFloor = (byte)MaxAllowableIRSignalFloorTB.Value;
            MyAutoNV.MsgStruct.MinAllowableIRSignalStrength = (byte)MinAllowableSignalStrengthTB.Value;
            MyAutoNV.MsgStruct.IRFaultTickLimit = (byte)IRFaultTickLimitTB.Value;

            MsgToTx.Enqueue(MyAutoNV.ToByteArray());
        }

        private void DisableAutoSteeringCB_CheckedChanged(object sender, EventArgs e)
        {
            UpdateAutoNVParameters();
            if (DisableAutoSteeringCB.Checked == true)
                SteeringGainControlTB.Enabled = false;
            else
                SteeringGainControlTB.Enabled = true;
        }

        private void DisableAutoDriveCB_CheckedChanged(object sender, EventArgs e)
        {
            UpdateAutoNVParameters();
            UpdateTextLabels();

            if (DisableAutoDriveCB.Checked == true)
                DriveGainGainControlTB.Enabled = false;
            else
                DriveGainGainControlTB.Enabled = true;
        }

        private void IntegralGainTB_Scroll(object sender, EventArgs e)
        {
            UpdateAutoNVParameters();
            UpdateTextLabels();
        }

        private void ResetIntegralCB_CheckedChanged(object sender, EventArgs e)
        {
            

            if (ResetIntegralCB.Checked == true)
            {
                IntegralGainTB.Enabled = false;
                IntegralGainTB.Value = 0;
            }
            else
            {
                IntegralGainTB.Enabled = true;
            }

            UpdateTextLabels();
            UpdateAutoNVParameters();
        }

        public void PostAutoNVStateVariableHandler(FTFSmartCarMessages.PostAutoNVStateVariables MyAutoNVStateVariables)
        {

            switch (MyAutoNVStateVariables.MsgStruct.AutoNVState)
            {
                case FTFSmartCarMessages.AutoNVStates.AUTO_NV_IDLE:
                    AutoNVStateLabel.Text = "AutoNVState: AUTO_NV_IDLE";
                    break;

                case FTFSmartCarMessages.AutoNVStates.AUTO_NV_GO:
                    AutoNVStateLabel.Text = "AutoNVState: AUTO_NV_GO";
                    break;
                
                case FTFSmartCarMessages.AutoNVStates.AUTO_NV_START_DELAY:
                    AutoNVStateLabel.Text = "AutoNVState: AUTO_START_DELAY";
                    break;

                case FTFSmartCarMessages.AutoNVStates.AUTO_NV_STOP_DELAY:
                    AutoNVStateLabel.Text = "AutoNVState: AUTO_NV_STOP_DELAY";
                    break;

                default:
                    AutoNVStateLabel.Text = "AutoNVState: Unknown State [" + MyAutoNVStateVariables.MsgStruct.AutoNVState + "]";;
                    break;
            }

            AutoNVCentroidLabel.Text = "AutoNVCentroid : " + String.Format("{0:0.000}", (double)MyAutoNVStateVariables.MsgStruct.IRCentroid/(double)256) + "[FIXED_7_8] " + MyAutoNVStateVariables.MsgStruct.IRCentroid + "[SIGNED_WORD]";
            AutoNVCommandedSteeringLabel.Text = "AutoNVCommandSteering : " + String.Format("{0:0.000}", (double)MyAutoNVStateVariables.MsgStruct.CommandedSteering / (double)256) + "[FIXED_7_8] " + MyAutoNVStateVariables.MsgStruct.CommandedSteering + "[SIGNED_WORD]";
            AutoNVCommandedDriveLabel.Text = "AutoNVCommandDrive: " + String.Format("{0:0.000}", (double)MyAutoNVStateVariables.MsgStruct.CommandedDrive / (double)256) + "[FIXED_7_8] " + MyAutoNVStateVariables.MsgStruct.CommandedDrive + "[SIGNED_WORD]";

 
        }

        private void CentroidFilterLengthTB_Scroll(object sender, EventArgs e)
        {
            if (CentroidFilterLengthTB.Value > 32)
                CentroidFilterLengthTB.Value = 32;
            UpdateAutoNVParameters();
            UpdateTextLabels();

        }

        private void SystemControlForm_Load(object sender, EventArgs e)
        {
            UpdateAutoNVParameters();
            UpdateTextLabels();
            LoadFormSettings();

        }

        private void MinAllowableSignalStrengthTB_Scroll(object sender, EventArgs e)
        {
            UpdateAutoNVParameters();
            UpdateTextLabels();

        }

        private void MaxAllowableIRSignalFloorTB_Scroll(object sender, EventArgs e)
        {
            UpdateAutoNVParameters();
            UpdateTextLabels();

        }

        private void IRFaultTickLimitTB_Scroll(object sender, EventArgs e)
        {
            UpdateAutoNVParameters();
            UpdateTextLabels();

        }

        private void CentroidDifferentialGainTB_Scroll(object sender, EventArgs e)
        {
            UpdateAutoNVParameters();
            UpdateTextLabels();
        }

        void LoadFormSettings()
        {
            if (File.Exists("SystemControlForm.jsx") == false)
            {
                SaveFormSettings();
            }
            else
            {
                Serializer MyJSONSerializer = new Serializer(typeof(SystemControlSettings));
                StreamReader SR = new StreamReader("SystemControlForm.jsx");
                try
                {
                    SystemControlSettings MyControlSettings = (SystemControlSettings)MyJSONSerializer.Deserialize(SR);
                    SteeringGainControlTB.Value = MyControlSettings.SteeringGain;
                    DriveGainGainControlTB.Value = MyControlSettings.DriveGain;
                    MaxDriveSpeedTB.Value = MyControlSettings.MaxDriveSpeed;
                    IntegralGainTB.Value = MyControlSettings.IntegralGain;
                    CentroidFilterLengthTB.Value = MyControlSettings.CentroidFilterLength;
                    CentroidDifferentialGainTB.Value = MyControlSettings.CentroidDifferentialGain;
                    MaxAllowableIRSignalFloorTB.Value = MyControlSettings.MaxAllowableSignalFloor;
                    MinAllowableSignalStrengthTB.Value = MyControlSettings.MinAllowableSignalStrength;
                    DisableAutoDriveCB.Checked = MyControlSettings.DisableAutoDrive;
                    DisableAutoSteeringCB.Checked = MyControlSettings.DisableAutoSteering;
                    ResetIntegralCB.Checked = MyControlSettings.ResetIntegral;

                }
                catch
                {

                }

                SR.Close();

            }

        }

        public void SaveFormSettings()
        {
            StreamWriter FileOut = new StreamWriter("SystemControlForm.jsx", false);

            SystemControlSettings MyControlSettings = new SystemControlSettings();

            MyControlSettings.SteeringGain = SteeringGainControlTB.Value;
            MyControlSettings.DriveGain = DriveGainGainControlTB.Value;
            MyControlSettings.MaxDriveSpeed = MaxDriveSpeedTB.Value;
            MyControlSettings.IntegralGain = IntegralGainTB.Value;
            MyControlSettings.CentroidFilterLength = CentroidFilterLengthTB.Value;
            MyControlSettings.CentroidDifferentialGain = CentroidDifferentialGainTB.Value;
            MyControlSettings.MaxAllowableSignalFloor = MaxAllowableIRSignalFloorTB.Value;
            MyControlSettings.MinAllowableSignalStrength = MinAllowableSignalStrengthTB.Value;
            MyControlSettings.DisableAutoDrive = DisableAutoDriveCB.Checked;
            MyControlSettings.DisableAutoSteering = DisableAutoSteeringCB.Checked;
            MyControlSettings.ResetIntegral = ResetIntegralCB.Checked;

            Serializer MySerializer = new Serializer(typeof(SystemControlSettings));

            MySerializer.Serialize(MyControlSettings, FileOut);
            FileOut.Close();
        }
    
    }




    public class SystemControlSettings
    {
        int _SteeringGain;
        int _DriveGain;
        int _MaxDriveSpeed;
        int _IntegralGain;
        int _CentroidFilterLength;
        int _CentroidDifferentialGain;
        int _MaxAllowableSignalFloor;
        int _MinAllowableSignalStrength;
        int _IRFaultTickLimit;
        bool _DisableAutoDrive;
        bool _DisableAutoSteering;
        bool _ResetIntegral;

       public int SteeringGain
        {
           get{ return _SteeringGain;}
           set{ _SteeringGain = value;}
        }

       public int DriveGain
        {
            get { return _DriveGain; }
            set { _DriveGain = value; }
        }

        public int MaxDriveSpeed
        {
            get { return _MaxDriveSpeed; }
            set { _MaxDriveSpeed = value; }
        }

       public int IntegralGain
        {
            get { return _IntegralGain; }
             set { _IntegralGain = value; }
        }

       public int CentroidFilterLength
        {
            get { return _CentroidFilterLength; }
            set { _CentroidFilterLength = value; }
        }

       public int CentroidDifferentialGain
        {
            get { return _CentroidDifferentialGain; }
            set { _CentroidDifferentialGain = value; }
        }

       public int MaxAllowableSignalFloor
       {
           get { return _MaxAllowableSignalFloor; }
           set { _MaxAllowableSignalFloor = value; }
        }

       public int MinAllowableSignalStrength
        {
            get { return _MinAllowableSignalStrength; }
            set { _MinAllowableSignalStrength = value; }
        }

       public int IRFaultTickLimit
       {
           get { return _IRFaultTickLimit; }
           set { _IRFaultTickLimit = value; }
        }

       public bool DisableAutoDrive
        {
            get { return _DisableAutoDrive; }
            set { _DisableAutoDrive = value; }
        }

       public bool DisableAutoSteering
        {
            get { return _DisableAutoSteering; }
            set { _DisableAutoSteering = value; }
        }

       public bool ResetIntegral
       {
           get { return _ResetIntegral; }
           set { _ResetIntegral = value; }
       }

           


    }
}
